// @Author miaoqing
// @Date 2024/10/21 17:01:00
// @Desc
package logic

import (
	"core/command"
	"core/common"
	"core/component/function"
	"core/component/logger"
	"time"
)

func (robot *Robot) checkBackpacking() {
	if len(robot.backpack) == 0 {
		return
	}
	now := time.Now().Unix()
	for cmd, v := range robot.backpack {
		if now-v.SendTime < common.PackingTimeOut {
			continue
		}
		robot.sendMsg(cmd, v.Msg)
	}
}

func (robot *Robot) doWithMsgBackpackingHandler(data interface{}) {
	msgRecv := &common.BackPackingMsg{}
	err := function.MyUnmarshal(data, msgRecv)
	if err != nil {
		return
	}
	delete(robot.backpack, command.Command(msgRecv.BackpackCmd))
}

func (robot *Robot) doWithGoalResultHandler(data interface{}) {
	msgRecv := &common.GoalResult{}
	err := function.MyUnmarshal(data, msgRecv)
	if err != nil {
		return
	}
	logger.Infof("result :%v", msgRecv.Result)
}

func (robot *Robot) setGoalPoint(x, y, w float64) {
	robot.rosClient.sendGoal(x, y, w)
}

func (robot *Robot) setCmdVel(x, y, yaw float32) {
	//point := &common.GoalPoint{
	//	X:   x,
	//	Y:   y,
	//	Yaw: yaw,
	//}
	//robot.sendMsg(command.SetCmdVel, point)
}
